#include "TrackInitializer.h"
#include <algorithm>
#include <cmath>
#include "../comm_define.h"
#include "../UKF/sigma_points.h"
#include "../AlgoUtility/algo_util.h"
#include "MTTUtils.h"

#define DEFAULT_SCAN_CYCLE	1.88

#ifndef M_PI 
#define M_PI 3.141592653
#endif

TrackInitializeParam::TrackInitializeParam() {
	minVel_ms = 0; //最小速度
	maxVel_ms = Knot2Ms(65); //最大速度
	m_r_err_std = 30;//距离量测误差
	m_a_err_std = 0.3; //方位的量测误差
	thetamax = 10;//初始航迹时目标最大方位变化量
	dk = 9.21; //卡方分布99.99的分位点
	staticObjSharkVel = Knot2Ms(10);
	max_acc_ms2 = 3.0;
	track_formation_M = 2;
	track_formation_N = 3;
}


TrackInitializer::TrackInitializer(onTrackConfirmedAction action)
	:globalTraceID(0),
	curSecsPreCycle(2.0),
	TrackConfirmedAct(action),
	FlickerDetector(std::bind(&TrackInitializer::onTraceInitSuccess, this, std::placeholders::_1)) {
}
TrackInitializer::~TrackInitializer() {
}


//计算K时刻的量测噪声（距离、方位）在直角坐标系下(XY)的标准差
void TrackInitializer::RAerr2XYerr(double r, double a,
	double m_r_err_std, double m_a_err_std,
	double& x_err_std, double& y_err_std) {

	//算法参考《雷达数据处理与应用》何友 第一版 P26

	double RAD_A = ANG2RAD(a); //目标方位
	double RAD_a_err_std = ANG2RAD(m_a_err_std); //方位的误差

	Matrix2d A;
	A(0, 0) = sin(RAD_A);
	A(0, 1) = -1.0 * r * cos(RAD_A);
	A(1, 0) = cos(RAD_A);
	A(1, 1) = r * sin(RAD_A);

	Matrix2d RE = Matrix2d::Zero();
	RE(0, 0) = POW2(m_r_err_std);
	RE(1, 1) = POW2(RAD_a_err_std);

	Matrix2d R = A * RE * A.transpose();

	x_err_std = sqrt(R(0, 0));
	y_err_std = sqrt(R(1, 1));

}

void TrackInitializer::traceSplit(TentativeTrack& trace, std::vector<plotRecord>& assignedPlotVector) {
	if (assignedPlotVector.empty()) {
		trace.updateWnd(false);
	}
	else {
		//复制以当前航迹为航迹头的N-1条航迹
		for (size_t ti = 1; ti < assignedPlotVector.size(); ti++) {
			createNewTraceFromCloneAndAddPlot(trace.traceNo, assignedPlotVector.at(ti));
		}
		//修改原始航迹
		trace.plotSeq.push_back(assignedPlotVector.at(0));
		trace.updateWnd(true);
		if (trace.filterInited) {
			Vector2d z;
			z << assignedPlotVector.at(0).R, assignedPlotVector.at(0).A;
			trace.UKF.update(z);
		}

	}
}

double TrackInitializer::calcIncludedAngle(const plotRecord& plot_0, //第一个点
	const plotRecord& plot_1, //第二个
	const CCVBlockInfo_t& a_plot)
{
	double a = POW2(a_plot.X - plot_1.X) + POW2(a_plot.Y - plot_1.Y);
	double b = POW2(plot_1.X - plot_0.X) + POW2(plot_1.Y - plot_0.Y);
	double c = POW2(a_plot.X - plot_0.X) + POW2(a_plot.Y - plot_0.Y);
	//使用三角形的点乘公式计算新点迹与之前航迹的夹角
	double alpha = acos((a + b - c) / (2 * sqrt(a * b)));
	alpha = 180 - alpha * 180 / M_PI;
	return alpha;
}
MatrixXd TrackInitializer::GetP(const plotRecord& plot, double T)
{
	MatrixXd P = MatrixXd::Zero(4, 4);  //目标状态空间误差协方差
	P(0, 0) = POW2(plot.X_err_std);
	P(0, 1) = POW2(plot.X_err_std) / T;
	P(1, 0) = POW2(plot.X_err_std) / T;
	P(1, 1) = 2.0 * POW2(plot.X_err_std) / POW2(T);
	P(2, 2) = POW2(plot.Y_err_std);
	P(2, 3) = POW2(plot.Y_err_std) / T;
	P(3, 2) = POW2(plot.Y_err_std) / T;
	P(3, 3) = 2.0 * POW2(plot.Y_err_std) / POW2(T);
	return P;
}


plotRecord TrackInitializer::buildOnePlotRecord(const CCVBlockInfo_t& a_plot, int64_t current_timeSecFromEpoch) {
	plotRecord newPlot;
	newPlot.X = a_plot.X;
	newPlot.Y = a_plot.Y;
	newPlot.R = a_plot.Range;
	newPlot.A = a_plot.Azimuth;
	newPlot.SampleRange_m = a_plot.SampleRange_m;
	newPlot.current_timeSecFromEpoch = current_timeSecFromEpoch;

	//计算当前量测下，新的关联的点迹的XY的误差
	RAerr2XYerr(a_plot.RangeRefRadar, a_plot.AzimuthRefRadar,
		trackParam.m_r_err_std, trackParam.m_a_err_std,
		newPlot.X_err_std, newPlot.Y_err_std);

	return newPlot;
}

void TrackInitializer::updateAssociation(const CCVBlockVec_t& plots,
	double  T,
	int64_t current_timeSecFromEpoch,
	std::vector<size_t>& assignedStatus) {

	curSecsPreCycle = T;
	std::vector<size_t> assignedStatusClone = assignedStatus;

	//保存当前所有的临时航迹ID
	std::vector<TraceIDType> currentTraceIDs;
	for (auto Trace_it = Traces.begin(); Trace_it != Traces.end(); ++Trace_it) {
		currentTraceIDs.push_back(Trace_it->first);
	}

	//遍历所有已经存在的临时航迹，将新的点迹进行关联
	for (size_t Trace_index = 0; Trace_index < currentTraceIDs.size(); Trace_index++) {
		std::vector<plotRecord> assignedPlotVector;//当前临时航迹所关联的量测列表
		TraceIDType TID = currentTraceIDs.at(Trace_index);
		auto trace_iter = Traces.find(TID);
		if (trace_iter == Traces.end())
			continue;

		TentativeTrack& trace = trace_iter->second;
		trace.timeSecFromEpoch = current_timeSecFromEpoch;//更新时间戳
		if (trace.plotSize() == 1) {
			//处理第二帧
			//临时航迹中只存在一点,则使用圆形波门，关联最大速度门限内的所有点迹
			plotRecord& t_plot = trace.plotSeq.front(); //待关联的点迹	
			for (size_t k = 0; k < plots.size(); k++) {
				if (assignedStatus[k] > 0) //点迹已经被关联
					continue;

				const CCVBlockInfo_t& a_plot = plots.at(k);
				double dist_pow2 = POW2(a_plot.X - t_plot.X) + POW2(a_plot.Y - t_plot.Y);

				double min_dist_pow2 = POW2(trackParam.minVel_ms * T);
				double max_dist_pow2 = POW2((trackParam.maxVel_ms * 1.2) * T);

				if (dist_pow2 >= min_dist_pow2 && dist_pow2 <= max_dist_pow2) {
					//点迹在速度门限内，进行关联
					plotRecord newPlot = buildOnePlotRecord(a_plot, current_timeSecFromEpoch);
					assignedPlotVector.push_back(newPlot); //保存关联的点迹
					assignedStatusClone[k] += 1;  //标记这个plot已经被关联一次				
				}
			}//for
		}
		else {
			assert(trace.filterInited && "trace.filterInited");
			DistributionParam_t& predPos = trace.UKF.predict(T);
			double PredPosR = predPos.mean(0, 0);
			double PredPosA = predPos.mean(1, 0);
			size_t plot_size = trace.plotSeq.size();
			const plotRecord& plot_0 = trace.plotSeq.at(plot_size - 2); //第一帧
			const plotRecord& plot_1 = trace.plotSeq.at(plot_size - 1); //第二帧
			double vx = trace.UKF.x(1, 0);
			double vy = trace.UKF.x(3, 0);
			double VEL = sqrt(POW2(vx) + POW2(vy));
			double X_PrePos = PredPosR * sin(PredPosA / 180.0 * M_PI);
			double Y_PrePos = PredPosR * cos(PredPosA / 180.0 * M_PI);
			//double maxMoveDist = VEL * T * 5;
			predPos.P_Inverse = predPos.P.inverse();
			MatrixXd P_inverse = predPos.P_Inverse;

			for (size_t k = 0; k < plots.size(); k++) { //开始便利所有的航迹点探测关联
				if (assignedStatus[k] > 0)
					continue;

				const CCVBlockInfo_t& a_plot = plots.at(k);

				double max_rang_limit_gate = mtt::getMaxErr(PredPosR); //目标在当前距离下允许的最大误差
				if (max_rang_limit_gate < 300)
					max_rang_limit_gate = 300; 


				//这里对回波进行粗略判断，排除明显不可能关联的目标，否则计算压力比较大
				if (POW2(X_PrePos - a_plot.X) + POW2(Y_PrePos - a_plot.Y) > POW2(max_rang_limit_gate))
					continue;
				//计算点迹与预测位置的加权欧式距离
				Vector2d a_plot_Z;
				a_plot_Z << a_plot.Range, a_plot.Azimuth;
				VectorXd residual_z = RD_2D_residual_func(a_plot_Z, predPos.mean);
				double dk = residual_z.transpose() * P_inverse * residual_z;
				if (dk <= trackParam.dk) {
					bool inGate = false;
					/*
					注释:如果目标在近处，对起始目标进行严格检测
					在远处的目标放宽要求
					*/

					//设定一个距离门限，在门限内的目标进行严格检查.远距离目标由于回波差不进行严格检测
					const double minCheckRange = 20000.0;
					if (a_plot.RangeRefRadar <= minCheckRange) {
						if (VEL <= trackParam.staticObjSharkVel)
							inGate = true;
						else {
							//确认点迹航向是否满足约束
							double alpha = calcIncludedAngle(plot_0, plot_1, a_plot);
							if (alpha <= trackParam.thetamax)
								inGate = true;
						}
					}
					else {
						inGate = true; 
					}
			
					if (inGate) {
						plotRecord newPlot = buildOnePlotRecord(a_plot, current_timeSecFromEpoch);
						assignedPlotVector.push_back(newPlot); //保存关联的点迹
						assignedStatusClone[k] += 1;  //标记这个plot已经被关联一次
					}
				}
				
			}//for
		}
		//根据当前临时航迹关联的点迹进行临时航迹的分裂
		traceSplit(trace, assignedPlotVector);
	}
	TraceInitSuccessMap_t trace_sucess_map;
	DeleteTraceVec_t delete_trace_vec;

	//对所有已经关联的点迹进行整理
	clearUpTrace(current_timeSecFromEpoch, trace_sucess_map, delete_trace_vec);
	tidyTrace(trace_sucess_map, delete_trace_vec);

	//如果已经有两个量测点，开始初始化UKF
	initTrackFilter(T);

	//对没有关联的点迹，起始新的临时航迹
	createNewTraceFromUnassignedPlot(plots, assignedStatusClone, current_timeSecFromEpoch);
}

static bool TentativeTrackSortByErr(const TentativeTrack& lv, const TentativeTrack& rv) {
	return lv.UKF.residual_z_norm_value < rv.UKF.residual_z_norm_value;
}

void TrackInitializer::tidyTrace(TraceInitSuccessMap_t& trace_sucess_map, DeleteTraceVec_t& delete_trace_vec) {
	/*这里判断，如果此航迹与已经成功起批的航迹同属于与一个航迹Root的话，则删除与此root相同的所有航迹*/
	for (auto it = Traces.begin(); it != Traces.end(); ) {
		bool isRootDup = false;
		for (auto im = trace_sucess_map.begin(); im != trace_sucess_map.end(); im++) {
			if (im->first == it->second.root_trace_id) {
				isRootDup = true;
				break;
			}
		}
		if (isRootDup) {
			Traces.erase(it++);
		}
		else {
			++it;
		}
	}
	/*
	由于在成功起始的航迹中存在多条航迹同属于一个root节点的，则这些相投根节点的目标只保留一条最优航迹
	*/
	for (auto im = trace_sucess_map.begin(); im != trace_sucess_map.end(); im++) {
		std::vector<TentativeTrack>& TentativeTrackVec = im->second;
		auto bestElement = std::min_element(TentativeTrackVec.begin(), TentativeTrackVec.end(), TentativeTrackSortByErr);
		onTraceInitSuccess(*bestElement);
	}

}

void TrackInitializer::clearUpTrace(int64_t current_timeSecFromEpoch,
	TraceInitSuccessMap_t& trace_sucess_map,
	DeleteTraceVec_t& delete_trace_vec) {

	std::vector<TentativeTrack>	CancelTracesVec;

	for (auto it = Traces.begin(); it != Traces.end(); ) {
		TentativeTrack& trace = it->second;
		if (trace.winPtr == 1) {
			it++;//临时航迹继续等待后续的量测
		}
		else if (trace.winPtr == 2) {
			if (trace.checkWnd[1] == 0) {
				//进行了两次关联，但是第二次没有关联量测则取消临时航迹

				if (trace.clutterDense < 20)
					CancelTracesVec.push_back(trace);

				onCancelTrace(trace);
				Traces.erase(it++);
			}
			else {
				//两次量测已经关联,等待更多的点迹
				it++;
			}
		}
		else {
			if (trace.score() >= (trace.track_formation_M + 2)) {
				//满足M/N起始条件
				//onTraceInitSuccess(it->second);
				trace.type = TRACK_TYPE_NORMAL_INIT;
				TraceIDType rootID = it->second.root_trace_id;
				trace_sucess_map[rootID].push_back(it->second);
				Traces.erase(it++);
			}
			else {
				if (trace.winPtr >= trace.track_formation_N + 2) {
					//到达最大的假设跟踪步数
					//onCancelTrace(trace);
					if (trace.clutterDense < 20)
						CancelTracesVec.push_back(trace);

					delete_trace_vec.push_back(trace.traceNo);
					Traces.erase(it++);
				}
				else {
					it++;
				}
			}
		}
	}

	//spdlog::debug("CancelTracesVec Size = {0}", CancelTracesVec.size());
	//FlickerDetector.update(CancelTracesVec, current_timeSecFromEpoch);
}

void TrackInitializer::createNewTraceFromCloneAndAddPlot(TraceIDType fromID, plotRecord& plot) {
	auto it = Traces.find(fromID);
	if (it == Traces.end())
		return;
	TraceIDType NID = getNewID(); //新的ID批号
	Traces[NID] = it->second.clone(NID); //复制原航迹的一份拷贝
	Traces[NID].plotSeq.push_back(plot);
	Traces[NID].updateWnd(true);

	if (Traces[NID].filterInited) {
		Vector2d z;
		z << plot.R, plot.A;
		Traces[NID].UKF.update(z);
	}
}

void TrackInitializer::createNewTraceFromUnassignedPlot(
	const CCVBlockVec_t& plots,
	const std::vector<size_t>& assignedStatus,
	int64_t current_timeSecFromEpoch) {
	for (size_t i = 0; i < assignedStatus.size(); i++) {
		if (assignedStatus[i] != 0)
			continue;

		//如果量测位于非常严重的杂波区域中，则不进行任何的航迹起始
		if (plots[i].ClutterDense > VERY_SERIOUS_CLUTTER_DENSE)
			continue;

		//对于本次没有关联的目标起始新的临时航迹
		TraceIDType NID = getNewID();
		TentativeTrack  NewTrace(NID);
		NewTrace.setClutterDense(plots[i].ClutterDense);//杂波密度
		plotRecord newPlot = buildOnePlotRecord(plots[i], current_timeSecFromEpoch);
		NewTrace.plotSeq.push_back(newPlot);//添加量测点
		NewTrace.updateWnd(true);//更新滑窗
		NewTrace.filterInited = false;
		NewTrace.timeSecFromEpoch = current_timeSecFromEpoch;
		NewTrace.root_trace_id = NID;
		Traces[NID] = NewTrace;//增加航迹
	}
}

void TrackInitializer::onTraceInitSuccess(TentativeTrack& trace) {
	//检验是否为杂波
	if (!isClutter(trace)) {
		if (TrackConfirmedAct)
			TrackConfirmedAct(trace);
	}
}

void TrackInitializer::onCancelTrace(TentativeTrack& trace) {
}

bool TrackInitializer::isClutter(TentativeTrack& trace) {
	if (trace.type == TRACK_TYPE_FLICKER_INIT)
		return false;  //对于静态闪烁目标，不进行处理

	double vx = trace.UKF.x(1, 0);
	double vy = trace.UKF.x(3, 0);
	double x = trace.UKF.x(0, 0);
	double y = trace.UKF.x(2, 0);
	double VEL = sqrt(POW2(vx) + POW2(vy)); //速度
	double range = sqrt(POW2(x) + POW2(y)); //距离
	double MAX_XY_STD_ERR = mtt::getMaxErr(range); //目标在当前距离下允许的最大误差

	//速度校验.如果速度过大则认为杂波
	if (VEL > trackParam.maxVel_ms)
		return true;

	//位置不确定性校验
	//if (trace.UKF.residual_z_norm_value > 2.77) //卡方0.25
	//	return true;

	//误差检验
	double P_X = sqrt(trace.UKF.P(0, 0));
	double P_Y = sqrt(trace.UKF.P(2, 2));
	if (P_X > MAX_XY_STD_ERR || P_Y > MAX_XY_STD_ERR) {
		return true;
	}
		
	//PASS
	return false;
}

void TrackInitializer::setTrackInitializeParam(const TrackInitializeParam& param) {
	trackParam = param;
}
void TrackInitializer::initTrackFilter(double T) {
	for (auto it = Traces.begin(); it != Traces.end(); it++) {
		TentativeTrack& trace = it->second;
		if (trace.filterInited)
			continue;

		/*使用最初关联的两个量测,初始化滤波器*/
		const plotRecord& plot_0 = trace.plotSeq.front(); //第一帧
		const plotRecord& plot_1 = trace.plotSeq.back(); //第二帧

		//根据前两帧计算目标状态的估计值
		VectorXd X = VectorXd::Zero(4);
		X(0, 0) = plot_1.X;						//X
		X(1, 0) = (plot_1.X - plot_0.X) / T;	//Vx
		X(2, 0) = plot_1.Y;						//Y
		X(3, 0) = (plot_1.Y - plot_0.Y) / T;	//Vy

		//double VEL = sqrt(X(1, 0) * X(1, 0) + X(3, 0) * X(3, 0));

		MatrixXd P = GetP(plot_1, T);//初始协方差
		Matrix2d R = Matrix2d::Zero();
		R << POW2(trackParam.m_r_err_std), 0,
			0, POW2(trackParam.m_a_err_std);

		//初始化的过程噪声设置的大一些，偏重于量测
		double dt = T;
		double acc_var = POW2(0.2);
		MatrixXd Q = MatrixXd::Zero(4, 4);
		Q.block<2, 2>(0, 0) = Q_discrete_white_noise(2, dt, acc_var);
		Q.block<2, 2>(2, 2) = Q_discrete_white_noise(2, dt, acc_var);

		trace.UKF.x = X;
		trace.UKF.P = P;
		trace.UKF.R = R;
		trace.UKF.Q = Q;
		trace.UKF.dt = T;
		trace.filterInited = true;
	}
}

void TrackInitializer::updateTrackByPlot(TentativeTrack& trace, const CCVBlockInfo_t& plot, double T, int64_t current_timeSecFromEpoch)
{
    plotRecord record = buildOnePlotRecord(plot, current_timeSecFromEpoch);

    trace.plotSeq.push_back(record);
    trace.updateWnd(true);
    if (trace.filterInited) {
        Vector2d z = Vector2d::Zero();
        z << record.R, record.A;
        trace.UKF.predict(T);
        trace.UKF.update(z);
    }
    else if (trace.plotSeq.size() >= 2)
    {
        initTrackFilter(trace, T);
    }
}

bool TrackInitializer::confirmTrack(TentativeTrack& track)
{
    if (track.score() >= (track.track_formation_M + 2))
    {
        onTraceInitSuccess(track);

        return true;
    }
    else
    {
        return false;
    }
}

TentativeTrack TrackInitializer::addNewTrack(const CCVBlockInfo_t& plot, int64_t current_timeSecFromEpoch)
{
    //对于本次没有关联的目标起始新的临时航迹
    TraceIDType NID = getNewID();
    TentativeTrack  NewTrace(NID);
    NewTrace.setClutterDense(plot.ClutterDense);//杂波密度
    plotRecord newPlot = buildOnePlotRecord(plot, current_timeSecFromEpoch);
    NewTrace.plotSeq.push_back(newPlot);//添加量测点
    NewTrace.updateWnd(true);//更新滑窗
    NewTrace.filterInited = false;
    NewTrace.timeSecFromEpoch = current_timeSecFromEpoch;
    NewTrace.root_trace_id = NID;

    return NewTrace;
}

void TrackInitializer::initTrackFilter(TentativeTrack& trace, double T)
{
    if (trace.filterInited)
        return;

    /*使用最初关联的两个量测,初始化滤波器*/
    const plotRecord& plot_0 = trace.plotSeq.front(); //第一帧
    const plotRecord& plot_1 = trace.plotSeq.back(); //第二帧

    //根据前两帧计算目标状态的估计值
    VectorXd X = VectorXd::Zero(5);
    X(0, 0) = plot_1.X;		//X
    X(1, 0) = (plot_1.X - plot_0.X) / T;  //Vx
    X(2, 0) = plot_1.Y;		 //Y
    X(3, 0) = (plot_1.Y - plot_0.Y) / T;  //Vy
    X(4, 0) = 0; //w

    MatrixXd P = GetP(plot_1, T);//初始协方差
    Matrix2d R = Matrix2d::Zero();
    R << POW2(trackParam.m_r_err_std), 0,
        0, POW2(trackParam.m_a_err_std);

    //初始化的过程噪声设置的大一些，偏重于量测
    MatrixXd Q = MatrixXd::Identity(5, 5) * 50;

    trace.UKF.x = X;
    trace.UKF.P = P;
    trace.UKF.R = R;
    trace.UKF.Q = Q;
    trace.UKF.dt = T;
    trace.filterInited = true;
}